/*******************************************************************************
*   hone_markers : Publish a hone(skeleton) information of markers for RViz
*   License      : BSD License
*   Copyright(c) 2011 Yoonseok Pyo.  All rights reserved.
*******************************************************************************/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <visualization_msgs/Marker.h>


int main( int argc, char** argv )
{
    ros::init(argc, argv, "hone_markers");  // initialization
    ros::NodeHandle n;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("hone_markers", 1);

    ros::Rate r(60);                        // 60hz

    tf::TransformListener listener;

    tf::StampedTransform tf_hone;
    geometry_msgs::Point point_hone;
   
    std::string marker_name[15]={   "/hone/head",            "/hone/neck",        "/hone/torso",
                                    "/hone/left_shoulder",   "/hone/left_elbow",  "/hone/left_hand", 
                                    "/hone/right_shoulder",  "/hone/right_elbow", "/hone/right_hand",
                                    "/hone/left_hip",        "/hone/left_knee",   "/hone/left_foot",
                                    "/hone/right_hip",       "/hone/right_knee",  "/hone/right_foot"};

    while (ros::ok())
    {
        visualization_msgs::Marker hone_markers[15];

        for(int i=0; i < 15; i++)
        {
            hone_markers[i].header.frame_id        = "/hone/openni_depth_frame";
            hone_markers[i].header.stamp           = ros::Time::now();
            hone_markers[i].action                 = visualization_msgs::Marker::ADD;
            hone_markers[i].pose.orientation.w     = 1.0;
            hone_markers[i].ns                     = "hone_markers";
            hone_markers[i].id                     = i;
            hone_markers[i].type                   = visualization_msgs::Marker::SPHERE;
            hone_markers[i].scale.x                = 0.05;
            hone_markers[i].scale.y                = 0.05;
            hone_markers[i].scale.z                = 0.05;
            hone_markers[i].color.r                = 1.0;
            hone_markers[i].color.g                = 1.0;
            hone_markers[i].color.b                = 0.0;
            hone_markers[i].color.a                = 1.0;
            hone_markers[i].lifetime = ros::Duration(0);

            bool  check_transform = listener.waitForTransform("/hone/openni_depth_frame" , marker_name[i], ros::Time(0), ros::Duration(1));
            
            if(check_transform == true)
            { 
                listener.lookupTransform("/hone/openni_depth_frame", marker_name[i], ros::Time(0), tf_hone);
                
                hone_markers[i].pose.position.x = tf_hone.getOrigin().x();          // x-coordinate of hone 
                hone_markers[i].pose.position.y = tf_hone.getOrigin().y();          // y-coordinate of hone 
                hone_markers[i].pose.position.z = tf_hone.getOrigin().z();          // z-coordinate of hone 

                marker_pub.publish(hone_markers[i]);                                // publish 
            }
        }
        r.sleep();
    }
}




